﻿// (c) Copyright Microsoft Corporation.
// This source is subject to the Microsoft Public License (Ms-PL).
// Please see http://go.microsoft.com/fwlink/?LinkID=131993 for details.
// All other rights reserved.

using System;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Threading;
using System.Text;

namespace Coding4Fun.BoxingBots.Robot
{
    // roboteq next generation motor controller
	public class MotorController : IDisposable
	{
        public const int MaxMotorControllerSpeed = 1000;
		private readonly SerialPort _com;

		#region constructors

        public MotorController(int comport) : this(comport, true) { }

	    public MotorController(int comport, bool isComportEnabled) : this("COM" + comport, isComportEnabled) { }

		public MotorController(string comport, bool isComportEnabled) : this()
		{
            if (comport == null || comport == "COM0")
                return;

			_com = new SerialPort(comport, 115200, Parity.None, 8, StopBits.One) {Handshake = Handshake.None};
			IsComportEnabled = isComportEnabled;
		}

		// hiding default constructor
		private MotorController() { }
		#endregion
		#region idispose method
		public void Dispose()
		{
            Dispose(true);

            GC.SuppressFinalize(this);
		}

        protected virtual void Dispose(bool disposing)
        {
            if (_com == null)
                return;

            Close();
            _com.Dispose();
        }

	    #endregion

        public bool IsComportEnabled { get; set; }
		#region public methods
		
		const string MotorCommand = "!M {0} {1}";
        bool _isDoneWriting = true;

		public bool SetMotorSpeed(int channelMotor1, int channelMotor2)
		{
            if (!_isDoneWriting)
                return true;

            if (channelMotor1 > MaxMotorControllerSpeed)
                channelMotor1 = MaxMotorControllerSpeed;
            else if (channelMotor1 < -MaxMotorControllerSpeed)
                channelMotor1 = -MaxMotorControllerSpeed;

            if (channelMotor2 > MaxMotorControllerSpeed)
                channelMotor2 = MaxMotorControllerSpeed;
            else if (channelMotor2 < -MaxMotorControllerSpeed)
                channelMotor2 = -MaxMotorControllerSpeed;
            

			Write(string.Format(MotorCommand, channelMotor1, channelMotor2));

            _isDoneWriting = true;

            return true;
		}
        
		public void Open()
		{
            if (!IsComportEnabled)
				return;

			if (!_com.IsOpen)
			{
				_com.Open();
                _com.DiscardInBuffer();
                _com.DiscardOutBuffer();
                Thread.Sleep(100);
			}
		}

		public void Close()
		{
			if (!_com.IsOpen)
				return;

			SetMotorSpeed(0, 0);
			
			_com.Close();
		}
		#endregion
		#region private methods

        private DateTime _lastWrite = DateTime.Now;
        private readonly object _writeLock = new object();
		private void Write(string value)
		{
            try
            {
                lock (_writeLock)
                {
                    if (IsComportEnabled && (DateTime.Now - _lastWrite).TotalMilliseconds >= 33)
                    {
                        //_com.Write(value + Environment.NewLine);
                        var bytes = Encoding.UTF8.GetBytes(value + Environment.NewLine);

                        _com.BaseStream.Flush();
                        _com.BaseStream.Write(bytes, 0, bytes.Length);
                        _com.BaseStream.Flush();
                    }

                    _lastWrite = DateTime.Now;
                }
            }
            catch (IOException ex0)
            {
                Debug.WriteLine(ex0.ToString());
            }
		}


		#endregion
	}
}


